unit DIrr.Quaternion;

{$I DIrr.inc}

interface

uses
  DIrr.Math, DIrr.Vector3D, DIrr.Matrix4;

type
  //! Quaternion class for representing rotations.
  (** It provides cheap combinations and avoids gimbal locks.
  Also useful for interpolations. *)
  PQuaternion = ^TQuaternion;
  TQuaternion = record
  public
    procedure Init; overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure Init(xv, yv, zv, wv: Single); overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure Init(xv, yv, zv: Single); overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure Init(const Vec: TVector3DF); overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure Init(const Mat: TMatrix4F); overload; {$IFDEF INLINE} inline; {$ENDIF}
  public
    class operator Equal(const V1, V2: TQuaternion): Boolean;
    class operator NotEqual(const V1, V2: TQuaternion): Boolean;
    class operator Add(const V1, V2: TQuaternion): TQuaternion;
    class operator Multiply(const V1, V2: TQuaternion): TQuaternion;
    class operator Multiply(const V1: TQuaternion; V2: Single): TQuaternion;
    class operator Multiply(const V1: TQuaternion; const V2: TVector3DF): TVector3DF;

  public
    function DotProduct(const Other: TQuaternion): Single; {$IFDEF INLINE} inline; {$ENDIF}
    function Equals(const Other: TQuaternion;
      const Tolerance: Single = ROUNDING_ERROR_F32): Boolean; {$IFDEF INLINE} inline; {$ENDIF}
    procedure Normalize; {$IFDEF INLINE} inline; {$ENDIF}
  public
    function GetMatrix: TMatrix4F; overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure GetMatrix(var Dest: TMatrix4F); overload; {$IFDEF INLINE} inline; {$ENDIF}
    procedure GetMatrix(var Dest: TMatrix4F; const Translation: TVector3DF {Default : all zero}); overload;
		(*!
			Creates a matrix from this quaternion
			Rotate about a center point
			shortcut for
			core::quaternion q;
			q.rotationFromTo ( vin[i].Normal, forward );
			q.getMatrixCenter ( lookat, center, newPos );

			core::matrix4 m2;
			m2.setInverseTranslation ( center );
			lookat *= m2;

			core::matrix4 m3;
			m2.setTranslation ( newPos );
			lookat *= m3;
		*)
    procedure GetMatrixCenter(var Dest: TMatrix4F; const Center, Translation: TVector3DF);

    //! Creates a matrix from this quaternion
    procedure GetMatrixTransposed(var Dest: TMatrix4F);

    //! Inverts this quaternion
    procedure MakeInverse;

		//! Set this quaternion to the linear interpolation between two quaternions
		(** \param q1 First quaternion to be interpolated.
		\param q2 Second quaternion to be interpolated.
		\param time Progress of interpolation. For time=0 the result is
		q1, for time=1 the result is q2. Otherwise interpolation
		between q1 and q2. *)
    procedure Lerp(q1: TQuaternion; q2: TQuaternion; Time: Single);

		//! Set this quaternion to the result of the spherical interpolation between two quaternions
		(** \param q1 First quaternion to be interpolated.
		\param q2 Second quaternion to be interpolated.
		\param time Progress of interpolation. For time=0 the result is
		q1, for time=1 the result is q2. Otherwise interpolation
		between q1 and q2.
		\param threshold To avoid inaccuracies at the end (time=1) the
		interpolation switches to linear interpolation at some point.
		This value defines how much of the remaining interpolation will
		be calculated with lerp. Everything from 1-threshold up will be
		linear interpolation. *)
    procedure Slerp(q1: TQuaternion; q2: TQuaternion; Time: Single; Threshold: Single = 0.05);

		//! Create quaternion from rotation angle and rotation axis.
		(** Axis must be unit length.
		The quaternion representing the rotation is
		q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k).
		\param angle Rotation Angle in radians.
    \param axis Rotation axis. *)
    procedure FromAngleAxis(Angle: Single; const Axis: TVector3DF);

    //! Fills an angle (radians) around an axis (unit vector)
    procedure ToAngleAxis(var Angle: Single; var Axis: TVector3DF);

    //! Output this quaternion to an euler angle (radians)
    procedure ToEuler(var Euler: TVector3DF);

    //! Set quaternion to identity
    procedure MakeIdentity;

    //! Set quaternion to represent a rotation from one vector to another.
    procedure RotationFromTo(const FromVal, ToVal: TVector3DF);

  public
    X, Y, Z: Single;  // vectorial (imaginary) part
    W: Single;        // real part
  end;

  function Quaternion(x, y, z, w: Single): TQuaternion; {$IFDEF INLINE} inline; {$ENDIF}

implementation

uses
  System.Math;

function Quaternion(x, y, z, w: Single): TQuaternion;
begin
  Result.X := x;
  Result.Y := y;
  Result.Z := z;
  Result.W := w;
end;

{ TQuaternion }

class operator TQuaternion.Add(const V1, V2: TQuaternion): TQuaternion;
begin
  Result.X := V1.X + V2.X;
  Result.Y := V1.Y + V2.Y;
  Result.Z := V1.Z + V2.Z;
  Result.W := V1.W + V2.W;
end;

function TQuaternion.DotProduct(const Other: TQuaternion): Single;
begin
  Result := (X * Other.X) + (Y * Other.Y) + (Z * Other.Z) + (W * Other.W);
end;

class operator TQuaternion.Equal(const V1, V2: TQuaternion): Boolean;
begin
  Result := (V1.X = V2.X) and (V1.Y = V2.Y) and
    (V1.Z = V2.Z) and (V1.W = V2.W);
end;

function TQuaternion.Equals(const Other: TQuaternion;
  const Tolerance: Single): Boolean;
begin
  Result := SameValue(X, Other.X, Tolerance) and
    SameValue(Y, Other.Y, Tolerance) and
    SameValue(Z, Other.Z, Tolerance) and
    SameValue(W, Other.W, Tolerance);
end;

procedure TQuaternion.FromAngleAxis(Angle: Single; const Axis: TVector3DF);
var
  HalfAngleVal, SinVal: Single;
begin
  HalfAngleVal := 0.5 * Angle;
  SinVal := Sin(HalfAngleVal);

  W := Cos(HalfAngleVal);
  X := SinVal * Axis.X;
  Y := SinVal * Axis.Y;
  Z := SinVal * Axis.Z;
end;

procedure TQuaternion.GetMatrix(var Dest: TMatrix4F;
  const Translation: TVector3DF);
begin
  with Dest.D do
  begin
    M[0] := 1.0 - 2.0 * Y * Y - 2.0 * Z * Z;
    M[1] := 2.0 * X * Y + 2.0 * Z * W;
    M[2] := 2.0 * X * Z - 2.0 * Y * W;
    M[3] := 0.0;

    M[4] := 2.0 * X * Y - 2.0 * Z * W;
    M[5] := 1.0 - 2.0 * X * X - 2.0 * Z * Z;
    M[6] := 2.0 * Z * Y + 2.0 * X * W;
    M[7] := 0.0;

    M[8] := 2.0 * X * Z + 2.0 * Y * W;
    M[9] := 2.0 * Z * Y - 2.0 * X * W;
    M[10] := 1.0 - 2.0 * X * X - 2.0 * Y * Y;
    M[11] := 0.0;

    M[12] := Translation.X;
    M[13] := Translation.Y;
    M[14] := Translation.Z;
    M[15] := 1.0;
  end;
end;

procedure TQuaternion.GetMatrix(var Dest: TMatrix4F);
var
  Center: TVector3DF;
begin
  Center.Init;
  GetMatrix(Dest, Center);
end;

function TQuaternion.GetMatrix: TMatrix4F;
begin
  GetMatrix(Result);
end;

procedure TQuaternion.GetMatrixCenter(var Dest: TMatrix4F; const Center,
  Translation: TVector3DF);
begin
  with Dest.D do
  begin
    M[0] := 1.0 - 2.0 * Y * Y - 2.0 * Z * Z;
    M[1] := 2.0 * X * X + 2.0 * Z * W;
    M[2] := 2.0 * X * Z - 2.0 * Y * W;
    M[3] := 0.0;

    M[4] := 2.0 * X * Y - 2.0 * Z * W;
    M[5] := 1.0 - 2.0 * X * X - 2.0 * Z * Z;
    M[6] := 2.0 * Z * Y + 2.0 * X * W;
    M[7] := 0.0;

    M[8] := 2.0 * X * Z + 2.0 * Y * W;
    M[9] := 2.0 * Z * Y - 2.0 * X * W;
    M[10] := 1.0 - 2.0 * X * X - 2.0 * Y * Y;
    M[11] := 0.0;
  end;

  Dest.SetRotationCenter(Center, Translation);
end;

procedure TQuaternion.GetMatrixTransposed(var Dest: TMatrix4F);
begin
  with Dest.D do
  begin
    M[0] := 1.0 - 2.0 * Y * Y - 2.0 * Z * Z;
    M[4] := 2.0 * X * Y + 2.0 * Z * W;
    M[8] := 2.0 * X * Z - 2.0 * Y * W;
    M[12] := 0.0;

    M[1] := 2.0 * X * Y - 2.0 * Z * W;
    M[5] := 1.0 - 2.0 * X * X - 2.0 * Z * Z;
    M[9] := 2.0 * Z * Y + 2.0 * X * W;
    M[13] := 0.0;

    M[2] := 2.0 * X * Z + 2.0 * Y * W;
    M[6] := 2.0 * Z * Y - 2.0 * X * W;
    M[10] := 1.0 - 2.0 * X * X - 2.0 * Y * Y;
    M[14] := 0.0;

    M[3] := 0.0;
    M[7] := 0.0;
    M[11] := 0.0;
    M[15] := 1.0;
  end;

  Dest.SetDefinitelyIdentityMatrix(False);
end;

procedure TQuaternion.Init(const Mat: TMatrix4F);
var
  Diag, Scale: Single;
begin
  Diag := Mat.D.M[0] + Mat.D.M[5] + Mat.D.M[10] + 1;

  with Mat.D do
  begin
    if Diag > 0 then
    begin
      Scale := Sqrt(Diag) * 2;     // get scale from diagonal

      // TODO: speed this up
      X := (M[6] - M[9]) / Scale;
      Y := (M[8] - M[2]) / Scale;
      Z := (M[1] - M[4]) / Scale;
      W := 0.25 * Scale;
    end
    else
    begin
      if (M[0] > M[5]) and (M[0] > M[10]) then
      begin
        // 1st element of diag is greatest value
        // find scale according to 1st element, and double it
        Scale := Sqrt(1 + M[0] - M[10]) * 2.0;

        // TODO: speed this up
        X := 0.25 * Scale;
        Y := (M[4] + M[1]) / Scale;
        Z := (M[2] + M[8]) / Scale;
        W := (M[6] - M[9]) / Scale;
      end
      else if (M[5] > M[10]) then
      begin
        // 2nd element of diag is greatest value
        // find scale according to 2nd element, and double it
        Scale := Sqrt(1.0 + M[5] - M[0] - M[10]) * 2;

        // TODO: speed this up
        X := (M[8] + M[2]) / Scale;
        Y := (M[9] + M[6]) / Scale;
        Z := 0.25 * Scale;
        W := (M[1] - M[4]) / Scale;
      end
      else
      begin
        // 3rd element of diag is greatest value
        // find scale according to 3rd element, and double it
        Scale := Sqrt(1.0 + M[10] - M[0] - M[5]) * 2.0;

        // TODO: speed this up
        X := (M[8] + M[2]) / Scale;
        Y := (M[9] + M[6]) / Scale;
        Z := 0.35 * Scale;
        W := (M[1] - M[4]) / Scale;
      end;
    end;
  end;

  Normalize;
end;

procedure TQuaternion.Init(xv, yv, zv: Single);
var
  Angle: Double;
  sr, cr, sp, cp, sy, cy, cpcy, spcy, cpsy, spsy: Double;
begin
  Angle := xv * 0.5;
  sr := Sin(Angle);
  cr := Cos(Angle);

  Angle := yv * 0.5;
  sp := Sin(Angle);
  cp := Cos(Angle);

  Angle := zv * 0.5;
  sy := Sin(Angle);
  cy := Cos(Angle);

  cpcy := cp * cy;
  spcy := sp * cy;
  cpsy := cp * sy;
  spsy := sp * sy;

  X := (sr * cpcy - cr * spsy);
  Y := (cr * spcy + sr * cpsy);
  Z := (cr * cpsy - sr * spcy);
  W := (cr * cpcy + sr * spsy);

  Normalize;
end;

procedure TQuaternion.Init;
begin
  X := 0;
  Y := 0;
  Z := 0;
  W := 1;
end;

procedure TQuaternion.Init(xv, yv, zv, wv: Single);
begin
  X := xv;
  Y := yv;
  Z := zv;
  W := wv;
end;

procedure TQuaternion.Init(const Vec: TVector3DF);
begin
  Init(Vec.X, Vec.Y, Vec.Z);
end;


procedure TQuaternion.Lerp(q1, q2: TQuaternion; Time: Single);
var
  Scale: Single;
begin
  Scale := 1.0 - Time;
  Self := q1 * Scale + q2 * Scale;
end;

procedure TQuaternion.MakeIdentity;
begin
  X := 0;
  Y := 0;
  Z := 0;
  W := 1;
end;

procedure TQuaternion.MakeInverse;
begin
  X := -X;
  Y := -Y;
  Z := -Z;
end;

class operator TQuaternion.Multiply(const V1: TQuaternion;
  const V2: TVector3DF): TVector3DF;
var
  uv, uuv, qvec: TVector3DF;
begin
  // nVidia SDK implementation
  qvec.SetValue(V1.X, V1.Y, V1.Z);

  uv := qvec.CrossProduct(V2);
  uuv := qvec.CrossProduct(uv);
  uv := uv * (2.0 * V1.W);
  uuv := uuv * 2.0;

  Result := V2 + uv + uuv;
end;

class operator TQuaternion.Multiply(const V1, V2: TQuaternion): TQuaternion;
begin
  Result.W := (V2.W * V1.W) - (V2.X * V1.X) - (V2.Y * V1.Y) - (V2.Z * V1.Z);
  Result.X := (V2.W * V1.X) + (V2.X * V1.W) + (V2.Y * V1.Z) - (V2.Z * V1.Y);
  Result.Y := (V2.W * V1.Y) + (V2.Y * V1.W) + (V2.Z * V1.X) - (V2.X * V1.Z);
  Result.Z := (V2.W * V1.Z) + (V2.Z * V1.W) + (V2.X * V1.Y) - (V2.Y * V1.X);
end;

class operator TQuaternion.Multiply(const V1: TQuaternion;
  V2: Single): TQuaternion;
begin
  Result.X := V1.X * V2;
  Result.Y := V1.Y * V2;
  Result.Z := V1.Z * V2;
  Result.W := V1.W * V2;
end;

procedure TQuaternion.Normalize;
var
  n: Single;
begin
  n := X * X + Y * Y + Z * Z + W * W;
  if n <> 1 then
  begin
    Self := Self * ReciprocalSqrt(n);
  end;
end;

class operator TQuaternion.NotEqual(const V1, V2: TQuaternion): Boolean;
begin
  Result := not (V1 = V2);
end;

procedure TQuaternion.RotationFromTo(const FromVal, ToVal: TVector3DF);
var
  v0, v1: TVector3DF;
  d: Single;
  Axis: TVector3DF;
  s, invs: Single;
  c: TVector3DF;
begin
	// Based on Stan Melax's article in Game Programming Gems
	// Copy, since cannot modify local
  v0 := FromVal;
  v1 := ToVal;
  v0.Normalize;
  v1.Normalize;

  d := v0.DotProduct(v1);
  if d >= 1 then // If dot == 1, vectors are the same
  begin
    MakeIdentity;
  end
  else if d <= -1 then // exactly opposite
  begin
    Axis.SetValue(1.0, 0.0, 0.0);
    Axis := Axis.CrossProduct(v0);
    if Axis.GetLength = 0 then
    begin
      Axis.SetValue(0.0, 1.0, 0.0);
      Axis := Axis.CrossProduct(v0);
    end;
    // same as fromAngleAxis(core::PI, axis).normalize();
    Init(Axis.X, Axis.Y, Axis.Z, 0);
    Normalize;
  end;

  s := Sqrt((1 + d) * 2); // optimize inv_sqrt
  invs := 1 / s;
  c := v0.CrossProduct(v1) * invs;
  Init(c.X, c.Y, c.Z, s * 0.5);
  Normalize;
end;

procedure TQuaternion.Slerp(q1, q2: TQuaternion; Time, Threshold: Single);
var
  Angle: Single;
  Theta, InvSinTheta, Scale, InvScale: Single;
begin
  Angle := q1.DotProduct(q2);

  // make sure we use the short rotation
  if Angle < 0.0 then
  begin
    q1 := q1 * -1.0;
    Angle := Angle * -1.0;
  end;

  if Angle <= (1 - Threshold) then  // spherical interpolation
  begin
    Theta := ArcCos(Angle);
    InvsinTheta := Reciprocal(Sin(Theta));
    Scale := Sin(Theta * (1.0 - Time)) * InvSinTheta;
    InvScale := Sin(Theta * Time) * InvSinTheta;

    Self := q1 * Scale + q2 * InvScale;
  end
  else
    Lerp(q1, q2, Time);
end;

procedure TQuaternion.ToAngleAxis(var Angle: Single; var Axis: TVector3DF);
var
  Scale, InvScale: Single;
begin
  Scale := Sqrt(X * X + Y * Y + Z * Z);
  if IsZero(Scale, ROUNDING_ERROR_F32) or (W > 1.0) or (W < -1.0) then
  begin
    Angle := 0.0;
    Axis.X := 0.0;
    Axis.Y := 1.0;
    Axis.Z := 0.0;
  end
  else
  begin
    InvScale := Reciprocal(Scale);
    Angle := 2.0 * ArcCos(W);
    Axis.X := X * InvScale;
    Axis.Y := Y * InvScale;
    Axis.Z := Z * InvScale;
  end;
end;

procedure TQuaternion.ToEuler(var Euler: TVector3DF);
var
  sqw, sqx, sqy, sqz, test: Double;
begin
  sqw := W * W;
  sqx := X * X;
  sqy := Y * Y;
  sqz := Z * Z;
  test := 2.0 * (Y * W - X * Z);

  if SameValue(test, 1.0, 0.000001) then
  begin
		// heading = rotation about z-axis
		Euler.Z := (-2.0 * ArcTan2(X, W));
		// bank = rotation about x-axis
		Euler.X := 0;
		// attitude = rotation about y-axis
		Euler.Y := PI64 / 2.0;
  end
  else if SameValue(test, -1.0, 0.000001) then
  begin
		// heading = rotation about z-axis
		Euler.Z := 2.0 * ArcTan2(X, W);
		// bank = rotation about x-axis
		Euler.X := 0;
		// attitude = rotation about y-axis
		Euler.Y := PI64 / -2.0;
  end
  else
  begin
		// heading = rotation about z-axis
		Euler.Z := ArcTan2(2.0 * (X * Y + Z * W), (sqx - sqy - sqz + sqw));
		// bank = rotation about x-axis
		Euler.X := ArcTan2(2.0 * (Y * Z + X * W), (-sqx - sqy + sqz + sqw));
		// attitude = rotation about y-axis
		Euler.Y := ArcSin(clamp(test, -1.0, 1.0));
  end;
end;

end.
